//result 中的xy是左上角的坐标
#pragma once

#include "../include/common.hpp"
#include "../include/detection.hpp"
#include "./track.cpp"
#include <cmath>
#include <fstream>
#include <iostream>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <algorithm> 

using namespace cv;
using namespace std;

#define SQUARE 16

#define RES_ALLOW 1
#define ENABLE_VALUE (ROWSIMAGE/2)
#define ENTERDOWN_VALUE (SQUARE*2)
#define ENTERUP_UPVALUE (SQUARE*1)
#define ENTERUP_DOWNVALUE   (ROWSIMAGE*2/3)
#define CRUISE_VALUE (ROWSIMAGE/2)
#define CRUISE_LEFT (COLSIMAGE/2-SQUARE*3)
#define CRUISE_RIGHT (COLSIMAGE/2+SQUARE*3)
#define EXIT_VALUE (ROWSIMAGE/2)
#define EXIT_LEFT (COLSIMAGE/2-SQUARE*2)
#define EXIT_RIGHT (COLSIMAGE/2+SQUARE*2)
#define EXIT_EFFECTIVE (ROWSIMAGE/3)

#define RES_DELAY (750)
#define RES_OUT_DELAY (700)
#define STOP_DELAY (1000)

class Rescue
{
private:
    long start_time;
    long end_time;
    //检测到入库标志次数
    int tumble=0;
    int patient=0;
    int evil=0;
    int thief=0;
    
    enum STEP
    {
        None=0,
        Enable,
        EnterDown,    // 进站
        EnterUP,
        Cruise,   // 巡航
        Stop,     // 停车
        Exit      // 出站
    };

    //左右入库
    bool enter_l=0;
    bool enter_r=0;

    //左右锥桶位置
    vector<POINT> pointConeLeft;
    vector<POINT> pointConeRight;

    //取出左上角的坐标
    //从detection结果里面捞出来锥桶坐标
    void get_cone(vector<PredictResult> predict)
    {
        pointConeLeft.clear();
        pointConeRight.clear();

        for(int i=0;i<predict.size();i++)
        {
            if (predict[i].type == LABEL_CONE) // 锥桶检测
            {
//赛曙坐标系是左手系，yolo坐标系竖着是y横着是x
                //屏幕左右部分划分
                if ((predict[i].x + predict[i].width / 2) < COLSIMAGE / 2)
                {
                    pointConeLeft.push_back(POINT(predict[i].y,predict[i].x));
                }else{
                    pointConeRight.push_back(POINT(predict[i].y, predict[i].x));
                }
            }
        }
    }
    //找到最低点，左右enter告诉的
    int get_lowest()
    {
        int lowest=0;
        if(enter_l==true)
        {
            for(int i=0;i<pointConeLeft.size();i++)
            {
                if(pointConeLeft[i].x>=lowest)
                {
                    lowest=pointConeLeft[i].x;
                }
            }  
        }
        if(enter_r==true)
        {
            for(int i=0;i<pointConeRight.size();i++)
            {
                if(pointConeRight[i].x>=lowest)
                {
                    lowest=pointConeRight[i].x;
                }
            }  
        }
        return lowest;
    }
    //分左右找到最高点
    int get_highest()
    {
        int highest=COLSIMAGE;
        if(enter_l==true)
        {
            for(int i=0;i<pointConeLeft.size();i++)
            {
                if(pointConeLeft[i].x<=highest)
                {
                    highest=pointConeLeft[i].x;
                }
            }  
        }
        if(enter_r==true)
        {
            for(int i=0;i<pointConeRight.size();i++)
            {
                if(pointConeRight[i].x<=highest)
                {
                    highest=pointConeRight[i].x;
                }
            }  
        }
        return highest;
    }
    //找出所有的锥桶
    vector<POINT> get_all_cone(vector<PredictResult> predict)
    {
        vector<POINT> cone;
        POINT cone_i;
        for(int i=0;i<predict.size();i++)
        {
            if (predict[i].type == LABEL_CONE) // 锥桶检测
            {
                cone_i.y=predict[i].x;
                cone_i.x=predict[i].y;
                cone.push_back(cone_i);
            }
        }
        return cone;
    }
    //对锥桶排序（从左到右）
    vector<POINT> cone_sort(vector<POINT> cone)
    {
        vector<POINT> cone_sorted;
        bool inserted = false; 
        if(!cone.empty())
        {
            cone_sorted.push_back(cone[0]);
            for(int i=1;i<cone.size();i++)
            {
                for(int j=0;j<cone_sorted.size();j++)
                {
                    inserted = false; 
                    if(cone[i].y<cone_sorted[j].y)
                    {
                        cone_sorted.insert((cone_sorted.begin()+j),cone[i]);
                        inserted = true;
                        break;
                    }
                }
                if (!inserted)
                {
                    cone_sorted.push_back(cone[i]);
                }
            }
        }
        return cone_sorted;
    }
    // vector<POINT> _cone_sort_(vector<PredictResult> predict)
    // {
    //     vector<POINT> cone;
    //     POINT cone_i;
    //     for(int i=0;i<predict.size();i++)
    //     {
    //         if(predict[i].type==LABEL_CONE)
    //         {
    //             cone_i.x=predict[i].y;
    //             cone_i.y=predict[i].x;
    //             if(cone.size()==0)
    //             {
    //                 cone.push_back(cone_i);
    //             }else{
    //                 for(int j=0;j<cone.size();j++)
    //                 {
    //                     if(cone[j].y>cone_i.y)
    //                     {
    //                         cone.insert((cone.begin()+j),cone_i);
    //                     }
    //                 }
    //             }
    //         }
    //     }
    //     return cone;
    // }
    
    //锥桶单调性,0不单调1单增-1单减,cone所有锥桶的位置(等高视为不单调)
    int cone_mono(vector<POINT> cone)
    {
        int mono=0;
        int last_mono=0;
        if(cone.size()>1)
        {
            for(int i=1;i<cone.size();i++)
            {
                if(cone[i-1].x==cone[i].x){
                    mono=0;
                }else if(cone[i-1].x<cone[i].x){
                    mono=1;
                }else if(cone[i-1].x>cone[i].x){
                    mono=-1;
                }
                if(i==1)
                {
                    last_mono=mono;
                }
                if(last_mono!=mono)
                {
                    mono=0;
                }
                last_mono=mono;
            }
        }
        return mono;
    }
    //锥桶是否都在规定位置以下
    bool cone_below(vector<POINT> cone)
    {
        bool below=true;
        for(int i=0;i<cone.size();i++)
        {
            if(cone[i].x<CRUISE_VALUE)
            {
                below=false;
            }
        }
        return below;
    }
    bool cone_upon(vector<POINT> cone)
    {
        bool upon=true;
        for(int i=0;i<cone.size();i++)
        {
            if(cone[i].x>EXIT_VALUE)
            {
                upon=false;
            }
        }
        return upon;
    }
public:
    STEP step;

    //初始化
    void rescue_init()
    {
        tumble=0;
        thief=0;
        evil=0;
        patient=0;
        step=STEP::None;
        enter_l=false;
        enter_r=false;
    }

    //查找标牌如果在一定次数之内大于ALLOW就可以判定为入库
    //判断的标志位直接在private里
    bool if_rescue(vector<PredictResult> predict)
    {
        for(int i=0;i<predict.size();i++)
        {
            if (predict[i].type == LABEL_TUMBLE ) // 伤员平民标志检测
            {
                tumble++;
                break;
            }
            if (predict[i].type == LABEL_PATIENT ) // 伤员平民标志检测
            {
                patient++;
                break;
            }
            if (predict[i].type == LABEL_EVIL ) // 伤员平民标志检测
            {
                evil++;
                break;
            }
            if (predict[i].type == LABEL_THIEF ) // 伤员平民标志检测
            {
                thief++;
                break;
            }
        }
        if(tumble==RES_ALLOW||patient==RES_ALLOW)
        {
            enter_l=true;
            step=Enable;
            return 1;
        }
        if(evil==RES_ALLOW||thief==RES_ALLOW)
        {
            enter_r=true;
            step=Enable;
            return 1;
        }
        return 0;
    }

    void process(track &track,vector<PredictResult> predict,bool back_flag)
    {
        std::cout<<step<<std::endl;
        int i=0;
        switch(step)
        {
            //不进行补线
            //最下方的锥桶进入屏幕下方
            case STEP::Enable:
            {
                //分别找出左右锥桶
                get_cone(predict);
                //直接找出位置最低的锥桶
                if(get_lowest()>=ENABLE_VALUE)
                {
                    step=STEP::EnterDown;
                }
            break;
            }
            //不做处理
            //最上方的锥桶出现的时候进下一阶段
            case STEP::EnterDown:
            {
                get_cone(predict);
                if(get_highest()<=ENTERDOWN_VALUE)
                {
                    step=STEP::EnterUP;
                }
            break;
            }
            //不做处理
            //找最高的锥桶，进入ENTERUPVALUE屏幕的时候右转
            case STEP::EnterUP:
            {
                get_cone(predict);
                int enter_up_position=get_highest();
                if((enter_up_position>=ENTERUP_UPVALUE)&&(enter_up_position<=ENTERUP_DOWNVALUE))
                {
                    step=STEP::Cruise;
                    start_time = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//start
                }
            break;
            }
            //补线(或者直接控制舵机pwm)
            //锥桶顶到头上停车（所有锥桶位于下半屏幕，单调+-）
            case STEP::Cruise:
            {
                end_time = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                if((end_time-start_time)>RES_DELAY)
                {
                    step=STEP::Stop;
                }
                //补线
                if(enter_l==true)
                {
                    for(i=0;i<ROWSIMAGE;i++)
                    {
                        track.r_border[i].y=CRUISE_LEFT;
                        track.l_border[i].y=0;
                    }
                }if(enter_r==true)
                {
                    for(i=0;i<ROWSIMAGE;i++)
                    {
                        track.r_border[i].y=COLSIMAGE;
                        track.l_border[i].y=CRUISE_RIGHT;
                    }
                }
                //在curisevalue下面，右增
                vector<POINT> cone_cruise = get_all_cone(predict);
                if(cone_below(cone_cruise)==true)
                {   
                    //c++自带升序排序函数
                    // std::sort(cone_cruise.begin(), cone_cruise.end(), [](const POINT& a, const POINT& b) {return a.y < b.y;});
                    cone_cruise=cone_sort(cone_cruise);
                    if(enter_l){
                        if(cone_mono(cone_cruise)==-1&&(end_time-start_time)>RES_OUT_DELAY)
                        {
                            step=STEP::Stop;
                            start_time = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//start
                        }
                    }else if(enter_r){
                        if(cone_mono(cone_cruise)==1&&(end_time-start_time)>RES_OUT_DELAY)
                        {
                            step=STEP::Stop;
                            start_time = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//start
                        }
                    }
                }
            break;
            }
            //停车,在main里面蜂鸣
            case STEP::Stop:
            {
                //补线
                if(enter_l==true)
                {
                    for(i=0;i<ROWSIMAGE;i++)
                    {
                        track.r_border[i].y=0;
                        track.l_border[i].y=CRUISE_LEFT;
                    }
                }if(enter_r==true)
                {
                    for(i=0;i<ROWSIMAGE;i++)
                    {
                        track.r_border[i].y=CRUISE_RIGHT;
                        track.l_border[i].y=COLSIMAGE;
                    }
                }
                end_time = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                if((end_time-start_time)>STOP_DELAY)
                {
                    step=STEP::Exit;
                    start_time = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//start
                }
            break;
            }
            //把上面压的栈一个一个弹出来就算完了
            case STEP::Exit:
            {    
                end_time = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                if((end_time-start_time)>RES_DELAY)
                {
                    step=STEP::None;
                }
                //补线
                if(enter_l==true)
                {
                    for(i=0;i<ROWSIMAGE;i++)
                    {
                        track.r_border[i].y=0;
                        track.l_border[i].y=CRUISE_LEFT;
                    }
                }if(enter_r==true)
                {
                    for(i=0;i<ROWSIMAGE;i++)
                    {
                        track.r_border[i].y=CRUISE_RIGHT;
                        track.l_border[i].y=COLSIMAGE;
                    }
                }
                //暂时用偏差
                // if(back_flag==true)
                // {
                //     step=STEP::None;
                // }
                //锥桶全在最上方
                vector<POINT> cone_exit = get_all_cone(predict);
                int cone_num_l=0;
                int cone_num_r=0;
                bool exit_flag=true;
                if(cone_upon(cone_exit)==true&&track.effictive_line_num>EXIT_EFFECTIVE)
                {
                    for(i=0;i<cone_exit.size();i++)
                    {
                        if(cone_exit[i].y<EXIT_LEFT){
                            cone_num_l++;
                        }else if(cone_exit[i].y>EXIT_RIGHT){
                            cone_num_r++;
                        }else{
                            exit_flag=false;
                        }
                    }
                }else{
                    exit_flag=false;
                }
                //中间没有锥桶，而且左右两边同时至少有一个锥桶
                if(exit_flag==true&&cone_num_l!=0&&cone_num_r!=0&&(end_time-start_time)>RES_OUT_DELAY)
                {
                    step=STEP::None;
                }

            break;
            }
        }
        return;
    }

    void draw(Mat & binary_mat)
    {
        if(step!=STEP::None)
        {
            putText(binary_mat,"RescueScene:"+to_string(step) , Point(COLSIMAGE/3,15),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
        }
    }
};